#include "include.h"

_MB_PID_T MB_PID;

//static void MeBsp_Led_Out_Pin(void);

void MeBsp_PID_Init(void)
{
  memset(&MB_PID,0,sizeof(_MB_PID_T));
  MB_PID.wKp = 300;
  MB_PID.wKi = 100;
  MB_PID.wKd = 100;
  
  USER_Debug_Out("--PID Init\r\n");
}

int32_t MeBsp_PID_GetVal(_MB_PID_T *pid,int32_t err_val)
{
  pid->wCurrErrorVal = err_val;
  
  pid->wIntegralVal += err_val; //计算积分项
  if( pid->wIntegralVal > 0xFFFFFFFF ) {
    pid->wIntegralVal = 0xFFFFFFFF;
  }
  else if( pid->wIntegralVal < -0xFFFFFFFF ) {
    pid->wIntegralVal = -0xFFFFFFFF;
  }
  pid->wDerivativeVal = pid->wCurrErrorVal - pid->wLastErrorVal; //计算微分项
  
  // 计算PID输出
  pid->wOutVal = (int64_t)((pid->wKp * pid->wCurrErrorVal) + (pid->wKi * pid->wIntegralVal) + (pid->wKd*pid->wDerivativeVal))/65535;
  if ( pid->wOutVal > 65535 ) {
     pid->wOutVal = 65535;
  }
  else if ( pid->wOutVal < 0 ) {
     pid->wOutVal = 0;
  }
  
  pid->wLastErrorVal =  pid->wCurrErrorVal;
  
  return pid->wOutVal;
}
uint16_t vlagj = 0;
uint16_t tag_vlagj = 0;


const uint16_t val_tag_tab[10]={655,0,655,0,1000,0,32767,0,65535,0};

void MeBsp_PID_Hanl(void)
{
  static uint16_t time_cnt = 0;
  static uint8_t num_cnt = 0;
  if ( ++time_cnt > 5000 ) {
    time_cnt = 0;
    if ( ++num_cnt >= 10 ) {
      num_cnt = 0;
    }
  }
  
  tag_vlagj = val_tag_tab[num_cnt];
  vlagj = MeBsp_PID_GetVal(&MB_PID,tag_vlagj - vlagj);
  
  
 // USER_Debug_Out("%d,%d\r\n",vlagj,tag_vlagj);
}
